/*
* @Description: Matrix IQ竞赛机器人PID控制常用技巧

* @Author: Yinshen Wang
* @Email: wangyinshen@zju.edu.cn
* @Date:   2024-03-21 02:40:30
*/

#ifndef DRIVETRAINIQ_PID_CONTROL_H
#define DRIVETRAINIQ_PID_CONTROL_H
#include <MatrixIQ_ESP32.h>
#include <ESP32Encoder.h>
#include <MPU6050_light.h>

/*
TASK_TYPE:
    0: pwm控制 
    1: vc控制走直线
    2: wc控制转弯
    3: 直线运动抵达目标点（给定距离、最大速度）
    4: 自转（旋转角度，最大角速度）
    5: 公转（公转半径，旋转角度，最大线速度）
    6: 直线运动抵达目标点（给定距离、最大速度、初速度、末速度）
    7: 公转（公转半径，旋转角度，最大线速度、初速度、末速度）
*/

enum TASK_TYPE {
    PWM_CONTROL       = 0,
    CONSTANT_V        = 1,
    CONSTANT_W        = 2,
    GO_FORWARD        = 3,
    ROTATION          = 4,
    REVOLUTION        = 5,
    GO_FORWARD_V2     = 6,
    REVOLUTION_V2     = 7,
};

class DrivetrainIQ_PID_Control {

    public:
        DrivetrainIQ_PID_Control(MatrixIQ_ESP32* matrix, MPU6050* mpu6050,\
                                 ESP32Encoder* left_encoder, ESP32Encoder *right_encoder, \
                                 int left_motor_id, int right_motor_id);

        ~DrivetrainIQ_PID_Control();
  
        void inverse_kinematics();
        void task_init(long cur_time);
        void incremental_pid_motor_id(int motor_id);
        void angle_loop_pid();
        void position_loop_pid();
        float get_desire_angle(long cur_time);
        float get_desire_vel(long cur_time);
        float get_desire_pos(long cur_time);

        void motor_run();
        void task_run(long cur_time);
        void task_reset();
        void update();
        bool is_task_complete();

        void pwmControl(int left_pwm, int right_pwm, float duration);
        void forward(float distance, float max_v);
        void forward(float distance, float max_v, float start_v, float end_v);
        void rotate(float degree, float max_w);
        void revolve(float r, float degree, float max_v);
        void revolve(float r, float degree, float max_v, float start_v, float end_v);
        void stop();

        MatrixIQ_ESP32* matrix;
        MPU6050* mpu6050;
        ESP32Encoder* left_encoder, *right_encoder;
        int left_motor_id, right_motor_id;

    private:
        TASK_TYPE task_type;
        float task_duration;
        long task_ts;

        float str_vel;
        float end_vel;
        float max_vel;
        float pos_x;
  
        float max_wc;
        int angle_degree;
        float angle_rad;
  
        float radius;

        float exec_wc = 0;
        float exec_vel = 0;

        // variables for states
        long left_encoder_start, left_encoder_current;
        long right_encoder_start, right_encoder_current;
        float angleZ_taskstart_degree, angleZ_current_degree;
        float angleZ_taskstart_rad, angleZ_current_rad;

        // parameters for motor control
        int motor_pwm_desire[2] = {0,0};
        int motor_pwm_measure[2] = {0,0};
        int motor_pwm_out[2] = {0,0};
        float motor_w_desire[2] = {0.0f,0.0f};
        float motor_w_measure[2] = {0.0f,0.0f};

        // parameters for encoder feedback
        long delta_encoder[2] = {0};
        long last_encoder[2] = {0};
        long current_update_t, last_update_t;

        // parameters for wheel pid
        float inc_Kp[2] = {0.9, 0.9};
        float inc_Ki[2] = {0.55, 0.55};
        float inc_Kd[2] = {0.0, 0.0};
        long pid_err_sum[2] = {0,0};
        int last_pid_err[2] = {0,0};
        int last_last_pid_err[2] = {0,0};

        // parameters for angleZ pid
        float Kp_angleZ = 20;
        float Ki_angleZ = 0.1;
        float Kd_angleZ = 0.0;
        float angleZ_desire_rad = 0, angleZ_measure_rad = 0;
        float angleZ_desire_degree = 0, angleZ_measure_degree = 0;
        float angleZ_err_sum_rad = 0, angleZ_last_err_rad = 0;

        // parameter for posX pid
        float Kp_posX = 5;
        float Ki_posX = 0.03;
        float Kd_posX = 0.0;
        float posX_err_sum = 0, posX_last_err = 0, posX_desire = 0, posX_measure = 0;

        // variables for bang-bang control
        float t1;
        float t2;
        float t3;
        float max_vel2;
        float str_vel2;
        float end_vel2;
        float ref_pos;
        int bangbang_type = -1;
        int dir = 1;

        int encoder_sampling_period = 20;
        int control_loop_period = 20;
        float WIDTH_OF_CAR = 0.142;
        float WHEEL_RADIUS = 0.028;
        float INV_WHEEL_RADIUS = 35.71;
        float Rad2Degree = 57.29578;
        float Degree2Rad = 0.017453292;
        int ENCODER_PER_CIRCEL = 32400;
        float Encode2Rad = 0.000193925469;
        float Bodyv2PWM = 408.213;
        float Bodyw2PWM = 28.98;
        float W2PWM_FACTOR = 11.43;  // ctrl_pwm = w * W2PWM_FACTOR 11.428
        int max_acc = 2;
        int max_angular_acc = 4;
};

#endif